#include "rotate_compute.h"

// 旋转角度计算旋转矩阵
Matrix3 new_rotation(double angle) {
    Matrix3 rotation_matrix = { .data = { { 0 } } };

    double radians = angle * PI / 180.0;
    double cos_val = cos(radians);
    double sin_val = sin(radians);

    rotation_matrix.data[0][0] = cos_val;
    rotation_matrix.data[0][1] = -sin_val;
    rotation_matrix.data[1][0] = sin_val;
    rotation_matrix.data[1][1] = cos_val;
    rotation_matrix.data[2][2] = 1.0;

    return rotation_matrix;
}

// 矩阵乘法计算
Matrix3 matrix_multiply(Matrix3* m1, Matrix3* m2) {
    Matrix3 result = { .data = { { 0 } } };

    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            for (int k = 0; k < 3; ++k) {
                result.data[i][j] += m1->data[i][k] * m2->data[k][j];
            }
        }
    }
    return result;
}

double get_fov(ComputeParams* params, size_t frame, bool use_fovs, double timestamp_ms, bool for_ui) {
    double fov_scale = params->fov_scale;
    if (params->fov_overview && use_fovs && !for_ui) {
        fov_scale += 1.0;
    } else {
        fov_scale += 0.0;
    }

    double fov;
    if (use_fovs) {
        fov = params->fovs[frame] * fov_scale;
    } else {
        fov = 1.0;
    }
    fov = (fov > 0.001) ? fov : 0.001;

    if (!for_ui) {
        fov *= ((double)params->width / (double)(params->output_width > 1 ? params->output_width : 1));
    }

    return fov;
}

double get_ratio(const ComputeParams* params) {
    double ratio = (double)params->width / (double)(params->output_width > 1 ? params->video_width : 1);
    return ratio;
}

// Matrix3 get_new_k(ComputeParams* params, Matrix3* camera_matrix, double fov) {
//     double horizontal_ratio = (params->lens_input_horizontal_stretch > 0.01) ? params->lens_input_horizontal_stretch : 1.0;
//     double img_dim_ratio = get_ratio(params) / horizontal_ratio;
//     double out_dim[2] = { params->output_width, params->output_height };

//     Matrix3 new_k = *camera_matrix;
//     new_k.data[0][0] = new_k.data[0][0] * img_dim_ratio / fov;
//     new_k.data[1][1] = new_k.data[1][1] * img_dim_ratio / fov;
//     new_k.data[0][2] = out_dim[0] / 2.0;
//     new_k.data[1][2] = out_dim[1] / 2.0;

//     return new_k;
// }

/*
 * 本方法预期获取camera的内参，在当前项目阶段项目中，计划 hard_code
 */
void get_lens_data_at_timestamp(/* arguments */) {
    // ...
}


/*
 * Function to perform rolling shutter correction
 * 当前阶段不对果冻效应做修正，此方法暂不实现
 */
void rolling_shutter_correction(/* arguments */) {

    return;
}

/*
 * 对图像应用修正矩阵 (affine warp)
 */
void new_rotation_image(double video_rotation) {
    //
    //

    return;
}



/*
 * 修正应用的总入口
 */
void at_timestamp_for_points(/* arguments */) {
    //todo: 1. 初始化
    int timestamp; /* 当前待处理帧的时间戳 */
    int scaled_fps; /* 当前 */
    double frame_angle;
    //
    Matrix3 km = new_rotation(frame_angle);

    //todo: 2. 准备本步本帧的关键参数 内参、外参、缩放、fov、图像中心点、(播放速率) 等
//    get_lens_data_at_timestamp(/* arguments */);
//    get_fov();
//    get_new_k();


    //todo: 3. 链表中获取当帧frame的地址
//    size_t frame = frame_at_timestamp(timestamp, scaled_fps);


    // todo: 4. 计算每行的矫正矩阵（果冻效应矫正，当前不做，只对整帧应用一个修正矩阵
//    rolling_shutter_correction(/* arguments */);

    // todo： 5. 生成最终修正矩阵
//    struct Matrix3 image_rotation = new_rotation_image(video_rotation);

    // todo: 6. 图像warp实现



}


